#include <tT.h>
#include <env.h>

#include "can.h"

can_listener_t *can_listeners[LISTENERS];
can_t can = {tt_object()};

/**
 * Helper function to get a free message box.
 *
 * This could be easy modifed to just a single variable since we don't remove 
 * any listeners after we have added them.
 */
static inline int8_t canGetFreeMB(void) {
  uint16_t *canen= (uint16_t *)&CANEN2;

  for (uint8_t i = 0; i < LISTENERS; i++) {
    if ( !(can_listeners[i] || *canen & (1 << i)) ) {
      return i;
    }
  }

  return CAN_OUT_OF_MB;
}

/**
 * Initilize the CAN controller
 */
void canInit(can_t *self, void *none) {

  // 250kbit/s @ 16MHz
  // Values take from documentation
  CANBT1 = 0x06;
  CANBT2 = 0x0C;
  CANBT3 = 0x37;

  // Reset all MessageBoxes / Pages
  for (uint8_t i = 0; i < LISTENERS; i++) {
    CANPAGE = i << 4;

    // Read-Write is needed
    CANSTMOB;
    CANSTMOB = 0;

    // CAN MOb Control and DLC Register
    CANCDMOB = 0;

    // Clear ID
    CANIDT4 = 0;
    CANIDT3 = 0;
    CANIDT2 = 0;
    CANIDT1 = 0;

    // Clear ID Mask
    CANIDM4 = 0;
    CANIDM3 = 0;
    CANIDM2 = 0;
    CANIDM1 = 0;

    // Clear the Data
    for (uint8_t j = 0; j < 8; j++) {
      CANMSG = 0;
    }
  }

  // Interrupt only on RX Complete
  // ENIT: Enable all Interrupts (Except for CAN Timer Overrun Interrupt)
  // ENRX: Enable Receive Interrupt
  CANGIE = 1 << ENIT | 1 << ENRX;

  // Enable int for all MB
  CANIE2 = 0xff;
  CANIE1 = 0x7f;

  // Enable CAN
  CANGCON |= 1 << ENASTB;
}

/**
 * Send a can message
 */
uint16_t canSend(can_t *self, can_message_t *msg) {

  int8_t free_mb;


  // Select a free messagebox
  free_mb = canGetFreeMB();
  if (free_mb < 0) {
    return CAN_FAIL;
  }

  // Disable interrupts
  CANGIE &= ~(1 << ENIT);

  CANPAGE = free_mb << 4;

  // Set mailboxen id and mask
  CANIDT2 = msg->id << 5;
  CANIDT1 = msg->id >> 3;
  CANIDM2 = 0;
  CANIDM1 = 0;

  // We don't send remote frame requests
  CANIDT4  = 0;

  // Set the data
  uint8_t i = 0;
  for (; i < msg->length; i++) {
    CANMSG = msg->data[i];
  }

  // Clear rest of the data
  for (; i < 8; i++) {
    CANMSG = 0x00;
  }

  CANCDMOB = msg->length;
  CANCDMOB |= 1 << CONMOB0;

  CANGIE |= 1 << ENIT;  // Enable can int

  return CAN_OK;
}

/**
 * Install a listener for a certain ID and MASK
 */
uint16_t canInstall(can_t *self, can_listener_t **listener) {

  int8_t free_mb;

  // Select a free messagebox
  free_mb = canGetFreeMB();
  if (free_mb < 0) {
    return CAN_FAIL;
  }

  can_listeners[free_mb] = *listener;

  // Disable interrupts
  CANGIE &= ~(1 << ENIT);

  CANPAGE = free_mb << 4;

  // Set ID and MASK
  CANIDT2 = (*listener)->id << 5;
  CANIDT1 = (*listener)->id >> 3;
  CANIDM2 = (*listener)->id_mask << 5;
  CANIDM1 = (*listener)->id_mask >> 3;

  // Mailbox in receivedmode
  CANCDMOB = 1 << CONMOB1;

  // Enable can int
  CANGIE |= 1 << ENIT;

  return CAN_OK;
}

/**
 *  Handle CanRX interrupt.
 *
 *  Can't be inlined because the stack corrupts
 */
void canRX(void) {
	


  CANPAGE = CANHPMOB;

  can_listener_t *listener = can_listeners[CANHPMOB >> 4];

  listener->msg.id = CANIDT1 << 3 | CANIDT2 >> 5;
  listener->msg.length = CANCDMOB & 0x0f;

  // Get the data
  // Some of the data in msg.data could be wrong, but since the msg.length is
  // used this won't cause any problems.
  for (uint8_t i = 0; i < listener->msg.length; i++) {
    listener->msg.data[i] = CANMSG;
  }

  TT_WITHIN(ENV_MSEC(1), listener->deadline, listener->obj, listener->method, &listener->msg);

  // Acknowledge the interrupt using read-modify-write as stated in manual
  CANSTMOB;
  CANSTMOB = 0;

  // According the the manual we need to put it in received mode again
  CANCDMOB = 1 << CONMOB1;
}

ENV_INTERRUPT(CAN_INT_vect, canRX);
